/**
 * 
 */
package ch.unizh.ifi.ailab.ai;

import org.apache.log4j.Logger;

import ch.unizh.ifi.ailab.config.ProgramSettings;
import ch.unizh.ifi.ailab.dataTypes.Command;
import ch.unizh.ifi.ailab.dataTypes.SensorData;
import ch.unizh.ifi.ailab.dataTypes.Vector2D;


/**
 * this class implements a range histogram obstacle avoidance algorithm after
 * katevas et al. 2004
 * 
 * @author hans jaeckle jaeckle@gmail.com
 * 
 */
public class SMCRangeHistogram extends SensorMotorCoupling {

	// properties
	private ProgramSettings myProp = ProgramSettings.getProgramSettings();

	// logging
	private Logger myLog = Logger.getLogger(AICore.class);

	// factors needed according to Katevas et all 2004
	private final double repulsiveFactor = 1.0;

	private final double attractiveFactor = 1.0;

	// sensor angles to calculate with, in rad!
	private final double angleS1 = Math.toRadians(myProp
			.getIntProperty("sensor1Angle"));

	private final double angleS2 = Math.toRadians(myProp
			.getIntProperty("sensor2Angle"));

	private final double angleS3 = Math.toRadians(myProp
			.getIntProperty("sensor3Angle"));

	private final double angleS4 = Math.toRadians(myProp
			.getIntProperty("sensor4Angle"));

	// koordinatensystem: x = horizontal, y = vertikal!!!
	// reference point is in the middle between the front wheels
	// wheelbase is 50cm, wheeldistance is 23cm rear, and 21cm front.
	// factor to switch cm to distance from sensors: cm * 0.55
	private Vector2D reference_1 = new Vector2D((-10.5 * 0.55), 0); // front-left

	private Vector2D reference_2 = new Vector2D((10.5 * 0.55), 0); // front-right

	private Vector2D reference_3 = new Vector2D((-11.5 * 0.55), (-20 * 0.55)); // rear-left

	private Vector2D reference_4 = new Vector2D((11.5 * 0.55), (-20 * 0.55)); // rear-right

	private int minDist = 10;

	public SMCRangeHistogram() {
		myLog.debug("smc: initilisiere range histogram smc.");
	}

	public void calculateMotionCommand(SensorData data, Command command)
			throws UnavoidableObjectException {
		Vector2D force = calculateDrivingForce(calculateAttractiveForce(data,
				command), calculateRepulsiveForce(data));
		calculateCommand(force, command);
	}

	/**
	 * calculates the repulsive force vector after Katevas et al. 2004
	 * 
	 * @param data
	 *            provides a Sensordata set with the current readings
	 * @return returns a 2d vector with the repulsive force
	 */
	private Vector2D calculateRepulsiveForce(SensorData data)
			throws UnavoidableObjectException {
		if (data.getIo2() <= minDist || data.getIo3() <= minDist) {
			throw new UnavoidableObjectException(
					"to close, cannot avoid obsacle. stopping.");
		} else {
			Vector2D pointForce1 = new Vector2D(calculatePointRepulse(data,
					reference_1));
			Vector2D pointForce2 = new Vector2D(calculatePointRepulse(data,
					reference_2));
			Vector2D pointForce3 = new Vector2D(calculatePointRepulse(data,
					reference_3));
			Vector2D pointForce4 = new Vector2D(calculatePointRepulse(data,
					reference_4));

			Vector2D repulse = new Vector2D(pointForce1);
			repulse.plus(pointForce2);
			repulse.plus(pointForce3);
			repulse.plus(pointForce4);

			myLog.info("smc: repulsive force: " + repulse.toString());
			return repulse;
		}
	}

	/**
	 * berechnet die repulsive force F(ri) f�r einen bestimmten punkt �ber alle
	 * sektoren
	 * 
	 * @param data
	 *            aktuelle sensordaten
	 * @param reference
	 *            vektor zum punkt ausgehend vom der 0-referenz
	 * @return repulsive force for point defined through reference
	 */
	private Vector2D calculatePointRepulse(SensorData data, Vector2D reference) {
		// this method would be nicer having more flexibility considering the
		// number of sensors

		// algorithm implemented after katevas et al. 2004
		Vector2D obstacleVector1 = new Vector2D(data.getIo1(), angleS1);
		Vector2D forceSector1 = new Vector2D(obstacleVector1);
		forceSector1.minus(reference);
		forceSector1.mult(repulsiveFactor
				/ (Math.round(obstacleVector1.getNorm()) ^ (5)));
		myLog.debug("smc: point reulsive force calc: "
				+ forceSector1.toString());

		Vector2D obstacleVector2 = new Vector2D(data.getIo2(), angleS2);
		Vector2D forceSector2 = new Vector2D(obstacleVector2);
		forceSector2.minus(reference);
		forceSector2.mult(repulsiveFactor
				/ (Math.round(obstacleVector2.getNorm()) ^ (5)));
		myLog.debug("smc: point reulsive force calc: "
				+ forceSector2.toString());

		Vector2D obstacleVector3 = new Vector2D(data.getIo3(), angleS3);
		Vector2D forceSector3 = new Vector2D(obstacleVector3);
		forceSector3.minus(reference);
		forceSector3.mult(repulsiveFactor
				/ (Math.round(obstacleVector3.getNorm()) ^ (5)));
		myLog.debug("smc: point reulsive force calc: "
				+ forceSector3.toString());

		Vector2D obstacleVector4 = new Vector2D(data.getIo4(), angleS4);
		Vector2D forceSector4 = new Vector2D(obstacleVector4);
		forceSector4.minus(reference);
		forceSector4.mult(repulsiveFactor
				/ (Math.round(obstacleVector4.getNorm()) ^ (5)));
		myLog.debug("smc: point reulsive force calc: "
				+ forceSector4.toString());

		Vector2D resultingPointForce = new Vector2D(forceSector1);
		resultingPointForce.plus(forceSector2);
		resultingPointForce.plus(forceSector3);
		resultingPointForce.plus(forceSector4);
		// failure in the paper: both, x AND y part of the vectors have to be
		// turned around:
		resultingPointForce.mult(-1);

		myLog.debug("smc: point reulsive force for "
				+ Math.round(reference.getXVal()) + ":"
				+ Math.round(reference.getYVal()) + " is: "
				+ resultingPointForce);

		return resultingPointForce;
	}

	private Vector2D calculateAttractiveForce(SensorData data, Command command) {
		Vector2D attract = new Vector2D(0, 0);

		// attract
		// .plus(calculateOneAttract(reference_1, calculateTarget(command)));
		// attract
		// .plus(calculateOneAttract(reference_2, calculateTarget(command)));
		// attract
		// .plus(calculateOneAttract(reference_3, calculateTarget(command)));
		// attract
		// .plus(calculateOneAttract(reference_4, calculateTarget(command)));

		return attract;
	}

	private Vector2D calculateTarget(Command command) {
		int direction = command.getDirection()
				- myProp.getIntProperty("dirCenter");
		int speed = command.getSpeed() - myProp.getIntProperty("speedStop");
		return new Vector2D(direction, speed);
	}

	private Vector2D calculatePointAttract() {
		return null;
	}

	private Vector2D calculateOneAttract(Vector2D reference, Vector2D target) {
		Vector2D attract;

		Vector2D path = new Vector2D(reference);
		path.minus(target);
		double distance = path.getNorm();

		if (distance == 0) {
			attract = new Vector2D(0, 0);
		} else {
			double factor_1 = attractiveFactor / distance;
			double force_1_x = factor_1
					* (target.getXVal() - reference.getXVal()) / distance;
			double force_1_y = factor_1
					* (target.getYVal() - reference.getYVal()) / distance;
			attract = new Vector2D(force_1_x, force_1_y);
		}
		myLog.debug("smc: attractive: norm: " + distance + " target: "
				+ target.toString() + " reference: " + reference.toString());

		return attract;
	}

	private Vector2D calculateDrivingForce(Vector2D attract, Vector2D repulse) {
		Vector2D force = new Vector2D(0, 0);

		force.plus(attract);
		force.plus(repulse);

		myLog.warn("smc: driving f: " + force.toString() + " repulsive f: "
				+ repulse.toString() + " attractive f: " + attract.toString());

		return force;
	}

	/**
	 * determines the command settings for the current forcevector
	 * 
	 * @param force
	 *            gives the current driving force
	 * @param command
	 *            holds the current command
	 * @throws UnavoidableObjectException
	 *             if avoidance can not be performed.
	 */
	private void calculateCommand(Vector2D force, Command command) {
		double maxForceX = -1.3;
		double maxForceY = -30;

		// steering:
		double steeringBase = force.getXVal() / maxForceX;
		int steeringSteps = myProp.getIntProperty("dirMax")
				- myProp.getIntProperty("dirCenter");
		int direction = myProp.getIntProperty("dirCenter")
				+ (int) Math.round(steeringSteps * steeringBase);
		command.setDirection(direction);

		// speed:
		double speedBase = force.getYVal() / maxForceY;
		int speedSteps = myProp.getIntProperty("speedMax")
				- myProp.getIntProperty("speedStop");
		int speed = myProp.getIntProperty("speedStop")
				+ (int) Math.round(speedSteps * speedBase);
		command.setSpeed(speed);

		myLog.warn("smc: command: " + command.toString());
	}
}
